clc;
clear all;
close all;

%% Load data from data file
load observe.dat
load model.dat

img_2d_set = observe;
real_3d_set = model;

[n2d d2] = size(img_2d_set);%Get number of point 2D
[n3d d3] = size(real_3d_set);%Get number of point 3D
N = min(n2d, n3d);%They suppose to be the same, otherwise, choose the min


%% Build up the linear equation for the camera calibration
%Equation Qm = 0
%In which, Q is the 2nx12 matrix built from n 3D-2D point pairs
%and m is the 12x1 matrix built from reshaping the projection matrix M (3x4)
O = zeros(1, 4);
Q = zeros(2*N, 12);
for i = 1:N
    pi3 = real_3d_set(i,:);
    Pi = [pi3 1];%convert to homogenious coordidate 1x4
    ui = img_2d_set(i,1);
    vi = img_2d_set(i,2);
    Q(2*(i-1) + 1,:) = [Pi O -ui*Pi];
    Q(2*(i-1) + 2,:) = [O Pi -vi*Pi];    
end

%% Solve the linear equation based on least MSE
%Notice: in this process, we just find the normaized solution m
%Summary the least MSE method:
%Step 1: Given matrix Q, find X = transpose(Q)*Q
%Step 2: Find eigenvalues lamda_i and eigenvectors ei coresspondingly
%Step 3: Find the min(lamda_i) = lamda_k
%Step 4: Solution is the eigenvector ek

%Detail
%Step 1: Given matrix Q, find X = transpose(Q)*Q
X = Q'*Q;
[V D] = eig(X);%V is the set of eigenvectors, D is the set of eigenvalues
m = V(:,1);%m is the eigenvector correspond with the minimum eigenvalue
%rs = norm(Q*m,2)^2;%recheck the solution

M = reshape(m, 4, 3)';

%% Verify the correctness of the camera calibration
A1 = M(1,1:3);%used to estimate parameters
A2 = M(2,1:3);%used to estimate parameters
A3 = M(3,1:3);
b  = M(1:3,3);%used to estimate parameters
rho = 1/norm(A3,2);
M = M*rho;
%load the test image
org_img = imread('test_image.bmp');
%show the original image
figure(1);imshow(org_img);

% test with sample data
row = 0;
row_count = 11*11*3;
%generate the 3D coordinate
T = zeros(row_count,3);
for i = 0:10
    for j = 0:10        
        T(row + 1,:) = [i,j,0];        
        T(row + 2,:) = [10, i, j];
        T(row + 3,:) = [i, 10, j];
        row = row + 3;
    end
end

UV = project(T,M);
test_img = draw_on_img(org_img,UV);
figure(2);imshow(test_img);

UV1 = project(real_3d_set,M);
test_img1 = draw_on_img(org_img,UV1);
figure(3);imshow(test_img1);

%% Based on nomalized solution m, estimate the intrinsic and extrinsic
%% parameters
%Estimate the intrinsic parameters
%u0, v0, theta, alpha, beta
u0 = (rho^2)*DOT(A1,A3);
v0 = (rho^2)*DOT(A2,A3);

A1xA3       =       cross(A1,A3);
A2xA3       =       cross(A2,A3);
cos_theta   =       -DOT(A1xA3,A2xA3)/(norm(A1xA3)*norm(A1xA3));
theta       =       acos(cos_theta);
alpha       =       (rho^2)*norm(A1xA3)*sin(theta);
beta        =       (rho^2)*norm(A2xA3)*sin(theta);
%Estimate the extrinsic parameters
%Rotation vector, translation vector
r1          =       (1/norm(A1xA3))*A1xA3;
r3          =       rho*A3;
r2          =       cross(r3, r1);
R           =       [r1;r2;r3];
K           =       [alpha -alpha*cos_theta u0;0 beta/sin(theta) v0;0 0 1];
t           =       rho*K\b;
%% show the result on message box
% line1 = sprintf('Intrisic Parameters: \n');
% line2 = sprintf('u0 \t    = \t %2.3d;\n',u0);
% line3 = sprintf('v0 \t   = \t %2.3d;\n',v0);
% line4 = sprintf('theta \t = \t %2.3d;\n',theta);
% line5 = sprintf('alpha \t=  \t %2.3d;\n',alpha);
% line6 = sprintf('beta  \t=  \t %2.3d;\n',beta);
% line7 = sprintf('Extrinsic Parameters: \n');
% line8 = sprintf('R     \t=\t %2.3d %2.3d %2.3d \n %2.3d %2.3d %2.3d \n %2.3d %2.3d %2.3d;\n',R);
% line9 = sprintf('t     \t=\t %2.3d %2.3d %2.3d;\n',t);
% message = [line1,line2,line3,line4,line5,line6,line7,line8,line9];
% uiwait(msgbox(message,'CAMERA CALIBRATION'));

%% Create video
%generate a path on which the cube moves on
x1_rail = 0:0.1:9;
y1_rail = zeros(size(x1_rail));
z1_rail = zeros(size(x1_rail));
rail1 = [x1_rail' y1_rail' z1_rail'];
x2_rail = 9 + y1_rail;
y2_rail = x1_rail;
z2_rail = z1_rail;
rail2 = [x2_rail' y2_rail' z2_rail'];
x3_rail = x2_rail;
y3_rail = 9 + y1_rail;
z3_rail = x1_rail;
rail3 = [x3_rail' y3_rail' z3_rail'];
rail = [rail1;rail2;rail3];
% generate a cubic coordinate in the real world
width = 1;
height = 1;
depth = 1;
[rail_rows rail_cols] = size(rail);
%With each move, we project the cube on the image
for i = 1:rail_rows
    cube3D = draw_cube(width, depth, height, rail(i,:));
    cube2D = project(cube3D,M);
    video_img = org_img;    
    frame(:,:,1) = draw_on_img(video_img, cube2D);     
    frame(:,:,2) = frame(:,:,1);
    frame(:,:,3) = frame(:,:,1);
    movie(i) = im2frame(frame);
    clear video_img;
end
%Flush the data to AVI file
movie2avi(movie,'Cube.avi');










